#ifndef VISUAL_ODOMETRY_H
#define VISUAL_ODOMETRY_H

#include "common_include.h"
#include "map.h"
#include "config.h"
#include "viewer.h"
#include <thread>
#include <mutex>
#include <opencv2/opencv.hpp>

namespace myslam
{
	class Viewer;
	class VisualOdometry
	{
	public:
		enum VOState
		{
			INITIALZING = -1,
			OK=0,
			LOST
		};

		VOState state_;

		Map* map_;
		Frame* ref_;
		Frame* curr_;

		std::vector<Frame*> mvAllFrame;

		Viewer* mpViewer;
		Camera* mp_camera;

		cv::Ptr<cv::ORB> orb_;

		std::vector<cv::Point3f> pts_3d_ref;
		std::vector<cv::Point3f> pts_3d_all;
		std::vector<cv::KeyPoint> keypoints_curr_;

		cv::Mat descriptors_curr_, descriptors_ref_;

		std::vector<cv::DMatch> feature_matches_;
		cv::Mat T_c_r_estimate;

		int frameid;

		int num_inliers_;        
		int num_lost_;     

		int num_of_features_;
		double scale_factor_;
		int level_pyramid_;
		float match_ratio_;
		int max_num_lost_;
		int min_inliers_;

		double key_frame_min_rot;   
		double key_frame_min_trans; 

	public:
		VisualOdometry(std::string strSettingPath, Viewer* pViewer);
		~VisualOdometry();

		cv::Mat Tracking(cv::Mat im, cv::Mat imD, double tframe);
	private:
		cv::Mat addFrame(Frame* frame);

		void ExtractORB();
		void featureMatching();
		void poseEstimationPnP();
		void setRef3DPoints();

		void addKeyFrame();
		bool checkEstimatedPose();
		bool checkKeyFrame();
	};
}
#endif